/*
  File:    Gradients.cpp
  Purpose: Adaptive routines to calculate gradients  

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>

using namespace std;

#include "../ImprovedMath.h"
#include "../Matrix.h"
#include "Polynomial.h"
#include "LeastSquares.h"
#include "GeometricData.h"
#include "Curvature.h"

/*
  Note: In testing, the 5-point one-sided gradient stencils didn't help much more 
        than the 4-point one-sided stencils. So, if they cause a problem, feel 
        free to toss them out.
*/

bool CompareSigns( double input1, double input2 )
{
 extern double _EPS;
 
 if( input1 > _EPS && input2 > _EPS )
 { return true; }
 if( input1 <= _EPS && input2 <= _EPS )
 { return true; }
 return false;
}

int IsNextToInterface( Matrix& LevelSet, int i, int j )
{
 extern double _EPS;
 
 if( *LevelSet(i,j) <= _EPS )
 { return 0; }
 
 if( *LevelSet(i+1,j  ) <= _EPS )
 { return 1; }
 if( *LevelSet(i+1,j+1) <= _EPS )
 { return 2; }
 if( *LevelSet(i  ,j+1) <= _EPS )
 { return 3; }
 if( *LevelSet(i-1,j+1) <= _EPS )
 { return 4; }
 if( *LevelSet(i-1,j  ) <= _EPS )
 { return 5; }
 if( *LevelSet(i-1,j-1) <= _EPS )
 { return 6; }
 if( *LevelSet(i  ,j-1) <= _EPS )
 { return 7; }
 if( *LevelSet(i+1,j-1) <= _EPS )
 { return 8; }
 return 0;
}

bool CalculateGradients( Matrix& Input, Matrix* pGradX, Matrix* pGradY )
{ 
 extern double dx;
 extern double dy;
 extern int Xnodes;
 extern int Ynodes;
 extern Matrix Phi;

 (*pGradX).SetSize( Phi.TellRows() , Phi.TellCols() );
 (*pGradY).SetSize( Phi.TellRows() , Phi.TellCols() );
 
 int i,j;
 for( j=1 ; j < Input.TellCols() - 1 ; j++ )
 {
  for( i=1 ; i < Input.TellRows() - 1 ; i++ )
  {
   // x-derivative
   bool Xdone = false;
   if( CompareSigns( *Phi(i-1,j) , *Phi(i,j) ) && CompareSigns( *Phi(i,j) , *Phi(i+1,j) ) )
   { *(*pGradX)(i,j) = ( *Input(i+1,j) - *Input(i-1,j) )/(2.0*dx); Xdone = true; }
   if( !Xdone )
   {
    if( i-4 >= 0 && !Xdone )
	{
     if( CompareSigns( *Phi(i-4,j) , *Phi(i,j) ) && 
	     CompareSigns( *Phi(i-3,j) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i-2,j) , *Phi(i,j) ) && 
 	     CompareSigns( *Phi(i-1,j) , *Phi(i,j) ) && !Xdone )
	 {
	  *(*pGradX)(i,j) = ( (25.0/12.0)*(*Input(i,j)) 
	                     -4.0*(*Input(i-1,j)) 
	                     +3.0*(*Input(i-2,j))
                         -(4.0/3.0)*(*Input(i-3,j))
                         +0.25*(*Input(i-4,j)) )/dx;
      Xdone = true;	
	 }
	}
	if( i+4 < Xnodes && !Xdone )
	{
     if( CompareSigns( *Phi(i+4,j) , *Phi(i,j) ) && 
	     CompareSigns( *Phi(i+3,j) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i+2,j) , *Phi(i,j) ) && 
 	     CompareSigns( *Phi(i+1,j) , *Phi(i,j) ) && !Xdone )
	 {
	  *(*pGradX)(i,j) = ( (-25.0/12.0)*(*Input(i,j)) 
	                     +4.0*(*Input(i+1,j)) 
	                     -3.0*(*Input(i+2,j))
                         +(4.0/3.0)*(*Input(i+3,j))
                         -0.25*(*Input(i+4,j)) )/dx;
      Xdone = true;	
	 }
	}
	if( i-3 >= 0 && !Xdone )
	{
     if( CompareSigns( *Phi(i-3,j) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i-2,j) , *Phi(i,j) ) && 
 	     CompareSigns( *Phi(i-1,j) , *Phi(i,j) ) && !Xdone )
     {
      *(*pGradX)(i,j) = ( (-1.0/3.0)*(*Input(i-3,j)) 
  	                    + 1.5*(*Input(i-2,j))
	                    - 3.0*(*Input(i-1,j)) 
		 			   + (11.0/6.0)*(*Input(i,j)) )/dx;
      Xdone = true;
	 }
    }
	if( i+3 < Xnodes && !Xdone )
	{
     if( CompareSigns( *Phi(i+3,j) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i+2,j) , *Phi(i,j) ) && 
	     CompareSigns( *Phi(i+1,j) , *Phi(i,j) ) && !Xdone )
     {
      *(*pGradX)(i,j) = ( (1.0/3.0)*(*Input(i+3,j)) 
	                    - 1.5*(*Input(i+2,j))
	                    + 3.0*(*Input(i+1,j)) 
		  	 		   - (11.0/6.0)*(*Input(i,j)) )/dx; 
      Xdone = true;
     }
	}
	if( i-2 >= 0 && !Xdone )
	{
     if( CompareSigns( *Phi(i-2,j) , *Phi(i,j) ) &&
         CompareSigns( *Phi(i-1,j) , *Phi(i,j) ) && !Xdone )
     {
      *(*pGradX)(i,j) = ( 0.5*(*Input(i-2,j)) 
	                    - 2.0*(*Input(i-1,j)) 
		   			   + 1.5*(*Input(i,j)) )/dx; 
      Xdone = true;
	 }
    }
	if( i+2 < Xnodes && !Xdone )
	{
     if( CompareSigns( *Phi(i+2,j) , *Phi(i,j) ) &&
         CompareSigns( *Phi(i+1,j) , *Phi(i,j) ) && !Xdone )
     {
      *(*pGradX)(i,j) = (- 0.5*(*Input(i+2,j)) 
	                     + 2.0*(*Input(i+1,j)) 
		  			    - 1.5*(*Input(i,j)) )/dx; 
      Xdone = true;
	 }
    }
    if( CompareSigns( *Phi(i-1,j) , *Phi(i,j) ) && !Xdone )
    { *(*pGradX)(i,j) = ( *Input(i,j) - *Input(i-1,j) )/dx; Xdone = true; }
    if( CompareSigns( *Phi(i+1,j) , *Phi(i,j) ) && !Xdone )
    { *(*pGradX)(i,j) = ( *Input(i+1,j) - *Input(i,j) )/dx; Xdone = true; }
    if( !Xdone )
    { 
	 *(*pGradX)(i,j) = 0.0; Xdone = true;
	}
   }
   
   // y-derivative
   bool Ydone = false;
   if( CompareSigns( *Phi(i,j-1) , *Phi(i,j) ) && CompareSigns( *Phi(i,j) , *Phi(i,j+1) ) )
   { *(*pGradY)(i,j) = ( *Input(i,j+1) - *Input(i,j-1) )/(2.0*dy); Ydone = true; }
   if( !Ydone )
   {
    if( j-4 >= 0 && !Ydone )
	{
     if( CompareSigns( *Phi(i,j-4) , *Phi(i,j) ) && 
	     CompareSigns( *Phi(i,j-3) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i,j-2) , *Phi(i,j) ) && 
 	     CompareSigns( *Phi(i,j-1) , *Phi(i,j) ) && !Ydone )
	 {
	  *(*pGradY)(i,j) = ( (25.0/12.0)*(*Input(i,j)) 
	                     -4.0*(*Input(i,j-1)) 
	                     +3.0*(*Input(i,j-2))
                         -(4.0/3.0)*(*Input(i,j-3))
                         +0.25*(*Input(i,j-4)) )/dy;
      Ydone = true;	
	 }
	} 
	if( j+4 < Ynodes && !Ydone )
	{
	 if( CompareSigns( *Phi(i,j+4) , *Phi(i,j) ) && 
	     CompareSigns( *Phi(i,j+3) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i,j+2) , *Phi(i,j) ) && 
 	     CompareSigns( *Phi(i,j+1) , *Phi(i,j) ) && !Ydone )
 	{
	  *(*pGradY)(i,j) = ( (-25.0/12.0)*(*Input(i,j)) 
	                     +4.0*(*Input(i,j+1)) 
	                     -3.0*(*Input(i,j+2))
                         +(4.0/3.0)*(*Input(i,j+3))
                         -0.25*(*Input(i,j+4)) )/dy;
      Ydone = true;	
	 }
	}
	if( j-3 >= 0 && !Ydone )
	{
     if( CompareSigns( *Phi(i,j-3) , *Phi(i,j) ) && 
         CompareSigns( *Phi(i,j-2) , *Phi(i,j) ) && 
  	     CompareSigns( *Phi(i,j-1) , *Phi(i,j) ) && !Ydone )
     {
      *(*pGradY)(i,j) = ( (-1.0/3.0)*(*Input(i,j-3))  
	                    + 1.5*(*Input(i,j-2))
	                    - 3.0*(*Input(i,j-1)) 
		 			   + (11.0/6.0)*(*Input(i,j)) )/dy;
      Ydone = true;					   
	 }
    }
	if( j+3 < Ynodes && !Ydone )
	{
     if( CompareSigns( *Phi(i,j+3) , *Phi(i,j) ) && 
        CompareSigns( *Phi(i,j+2) , *Phi(i,j) ) && 
	    CompareSigns( *Phi(i,j+1) , *Phi(i,j) ) && !Ydone )
     {
      *(*pGradY)(i,j) = ( (1.0/3.0)*(*Input(i,j+3)) 
   	                    - 1.5*(*Input(i,j+2))
	                    + 3.0*(*Input(i,j+1)) 
		 			   - (11.0/6.0)*(*Input(i,j)) )/dy; 
      Ydone = true;
	 }
    }
	if( j-2 >= 0 && !Ydone )
	{
     if( CompareSigns( *Phi(i,j-2) , *Phi(i,j) ) &&
         CompareSigns( *Phi(i,j-1) , *Phi(i,j) ) && !Ydone )
     {
      *(*pGradY)(i,j) = ( 0.5*(*Input(i,j-2)) 
	                    - 2.0*(*Input(i,j-1)) 
		  			   + 1.5*(*Input(i,j)) )/dy; 
      Ydone = true;
	 }
    }
	if( j+2 < Ynodes && !Ydone )
	{
     if( CompareSigns( *Phi(i,j+2) , *Phi(i,j) ) &&
         CompareSigns( *Phi(i,j+1) , *Phi(i,j) ) && !Ydone )
     {
      *(*pGradY)(i,j) = (- 0.5*(*Input(i,j+2)) 
	                     + 2.0*(*Input(i,j+1)) 
		  			    - 1.5*(*Input(i,j)) )/dy; 
      Ydone = true;
	 }
    }
    if( CompareSigns( *Phi(i,j-1) , *Phi(i,j) ) && !Ydone )
    { *(*pGradY)(i,j) = ( *Input(i,j) - *Input(i,j-1) )/dy; Ydone = true; }
    if( CompareSigns( *Phi(i,j+1) , *Phi(i,j) ) && !Ydone )
    { *(*pGradY)(i,j) = ( *Input(i,j+1) - *Input(i,j) )/dy; Ydone = true; }
    if( !Ydone )
    { *(*pGradY)(i,j) = 0.0; }  
   }
  }
  
 }
 return true;
}

bool CalculateEasyGradients( Matrix& Input, Matrix* pGradX, Matrix* pGradY )
{
 extern Matrix Phi;
 extern double dx;
 extern double dy;

 (*pGradX).SetSize( Phi.TellRows() , Phi.TellCols() );
 (*pGradY).SetSize( Phi.TellRows() , Phi.TellCols() );
 
 int i,j;
 for( j=1 ; j < Input.TellCols() - 1 ; j++ )
 {
  for( i=1 ; i < Input.TellRows() - 1 ; i++ )
  {
   *(*pGradX)(i,j) = ( *Input(i+1,j) - *Input(i-1,j) )/(2.0*dx);
   *(*pGradY)(i,j) = ( *Input(i,j+1) - *Input(i,j-1) )/(2.0*dy);
  }
 }
 return true;
}

bool CalculateGradients( Matrix& LevelSet, Matrix& Input, Matrix* pGradX, Matrix* pGradY )
{
 extern double dx;
 extern double dy;

 (*pGradX).SetSize( LevelSet.TellRows() , LevelSet.TellCols() );
 (*pGradY).SetSize( LevelSet.TellRows() , LevelSet.TellCols() );
 
 int i,j;
 for( j=1 ; j < Input.TellCols() - 1 ; j++ )
 {
  for( i=1 ; i < Input.TellRows() - 1 ; i++ )
  {
   // x-derivative
   bool Xdone = false;
   if( CompareSigns( *LevelSet(i-1,j) , *LevelSet(i,j) ) && 
       CompareSigns( *LevelSet(i,j)   , *LevelSet(i+1,j) ) )
   { *(*pGradX)(i,j) = ( *Input(i+1,j) - *Input(i-1,j) )/(2.0*dx); Xdone = true; }
   if( !Xdone )
   {
    if( CompareSigns( *LevelSet(i-4,j) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i-3,j) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i-2,j) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i-1,j) , *LevelSet(i,j) ) && !Xdone )
	{
	 *(*pGradX)(i,j) = ( (25.0/12.0)*(*Input(i,j)) 
	                    -4.0*(*Input(i-1,j)) 
	                    +3.0*(*Input(i-2,j))
                        -(4.0/3.0)*(*Input(i-3,j))
                        +0.25*(*Input(i-4,j)) )/dx;
     Xdone = true;	
	}
    if( CompareSigns( *LevelSet(i+4,j) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i+3,j) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i+2,j) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i+1,j) , *LevelSet(i,j) ) && !Xdone )
	{
	 *(*pGradX)(i,j) = ( (-25.0/12.0)*(*Input(i,j)) 
	                    +4.0*(*Input(i+1,j)) 
	                    -3.0*(*Input(i+2,j))
                        +(4.0/3.0)*(*Input(i+3,j))
                        -0.25*(*Input(i+4,j)) )/dx;
     Xdone = true;	
	}
    if( CompareSigns( *LevelSet(i-3,j) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i-2,j) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i-1,j) , *LevelSet(i,j) ) && !Xdone )
    {
     *(*pGradX)(i,j) = ( (-1.0/3.0)*(*Input(i-3,j)) 
  	                   + 1.5*(*Input(i-2,j))
	                   - 3.0*(*Input(i-1,j)) 
					   + (11.0/6.0)*(*Input(i,j)) )/dx;
     Xdone = true;
    }
    if( CompareSigns( *LevelSet(i+3,j) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i+2,j) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i+1,j) , *LevelSet(i,j) ) && !Xdone )
    {
     *(*pGradX)(i,j) = ( (1.0/3.0)*(*Input(i+3,j)) 
	                   - 1.5*(*Input(i+2,j))
	                   + 3.0*(*Input(i+1,j)) 
		 	 		   - (11.0/6.0)*(*Input(i,j)) )/dx; 
     Xdone = true;
    }
    if( CompareSigns( *LevelSet(i-2,j) , *LevelSet(i,j) ) &&
        CompareSigns( *LevelSet(i-1,j) , *LevelSet(i,j) ) && !Xdone )
    {
     *(*pGradX)(i,j) = ( 0.5*(*Input(i-2,j)) 
	                   - 2.0*(*Input(i-1,j)) 
		 			   + 1.5*(*Input(i,j)) )/dx; 
     Xdone = true;
    }
    if( CompareSigns( *LevelSet(i+2,j) , *LevelSet(i,j) ) &&
        CompareSigns( *LevelSet(i+1,j) , *LevelSet(i,j) ) && !Xdone )
    {
     *(*pGradX)(i,j) = (- 0.5*(*Input(i+2,j)) 
	                    + 2.0*(*Input(i+1,j)) 
		 			    - 1.5*(*Input(i,j)) )/dx; 
     Xdone = true;
    }
    if( CompareSigns( *LevelSet(i-1,j) , *LevelSet(i,j) ) && !Xdone )
    { *(*pGradX)(i,j) = ( *Input(i,j) - *Input(i-1,j) )/dx; Xdone = true; }
    if( CompareSigns( *LevelSet(i+1,j) , *LevelSet(i,j) ) && !Xdone )
    { *(*pGradX)(i,j) = ( *Input(i+1,j) - *Input(i,j) )/dx; Xdone = true; }
    if( !Xdone )
    { *(*pGradX)(i,j) = 0.0; Xdone = true;
	}
   }
   
   // y-derivative
   bool Ydone = false;
   if( CompareSigns( *LevelSet(i,j-1) , *LevelSet(i,j) ) && 
       CompareSigns( *LevelSet(i,j) , *LevelSet(i,j+1) ) )
   { *(*pGradY)(i,j) = ( *Input(i,j+1) - *Input(i,j-1) )/(2.0*dy); Ydone = true; }
   if( !Ydone )
   {
    if( CompareSigns( *LevelSet(i,j-4) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i,j-3) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i,j-2) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i,j-1) , *LevelSet(i,j) ) && !Ydone )
	{
	 *(*pGradY)(i,j) = ( (25.0/12.0)*(*Input(i,j)) 
	                    -4.0*(*Input(i,j-1)) 
	                    +3.0*(*Input(i,j-2))
                        -(4.0/3.0)*(*Input(i,j-3))
                        +0.25*(*Input(i,j-4)) )/dy;
     Ydone = true;	
	} 
	if( CompareSigns( *LevelSet(i,j+4) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i,j+3) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i,j+2) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i,j+1) , *LevelSet(i,j) ) && !Ydone )
	{
	 *(*pGradY)(i,j) = ( (-25.0/12.0)*(*Input(i,j)) 
	                    +4.0*(*Input(i,j+1)) 
	                    -3.0*(*Input(i,j+2))
                        +(4.0/3.0)*(*Input(i,j+3))
                        -0.25*(*Input(i,j+4)) )/dy;
     Ydone = true;	
	}
 
    if( CompareSigns( *LevelSet(i,j-3) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i,j-2) , *LevelSet(i,j) ) && 
 	    CompareSigns( *LevelSet(i,j-1) , *LevelSet(i,j) ) && !Ydone )
    {
     *(*pGradY)(i,j) = ( (-1.0/3.0)*(*Input(i,j-3))  
	                   + 1.5*(*Input(i,j-2))
	                   - 3.0*(*Input(i,j-1)) 
					   + (11.0/6.0)*(*Input(i,j)) )/dy;
     Ydone = true;					   
    }
    if( CompareSigns( *LevelSet(i,j+3) , *LevelSet(i,j) ) && 
        CompareSigns( *LevelSet(i,j+2) , *LevelSet(i,j) ) && 
	    CompareSigns( *LevelSet(i,j+1) , *LevelSet(i,j) ) && !Ydone )
    {
     *(*pGradY)(i,j) = ( (1.0/3.0)*(*Input(i,j+3)) 
   	                   - 1.5*(*Input(i,j+2))
	                   + 3.0*(*Input(i,j+1)) 
					   - (11.0/6.0)*(*Input(i,j)) )/dy; 
     Ydone = true;
    }
    if( CompareSigns( *LevelSet(i,j-2) , *LevelSet(i,j) ) &&
        CompareSigns( *LevelSet(i,j-1) , *LevelSet(i,j) ) && !Ydone )
    {
     *(*pGradY)(i,j) = ( 0.5*(*Input(i,j-2)) 
	                   - 2.0*(*Input(i,j-1)) 
		 			   + 1.5*(*Input(i,j)) )/dy; 
     Ydone = true;
    }
    if( CompareSigns( *LevelSet(i,j+2) , *LevelSet(i,j) ) &&
        CompareSigns( *LevelSet(i,j+1) , *LevelSet(i,j) ) && !Ydone )
    {
     *(*pGradY)(i,j) = (- 0.5*(*Input(i,j+2)) 
	                    + 2.0*(*Input(i,j+1)) 
		 			    - 1.5*(*Input(i,j)) )/dy; 
     Ydone = true;
    }
    if( CompareSigns( *LevelSet(i,j-1) , *LevelSet(i,j) ) && !Ydone )
    { *(*pGradY)(i,j) = ( *Input(i,j) - *Input(i,j-1) )/dy; Ydone = true; }
    if( CompareSigns( *LevelSet(i,j+1) , *LevelSet(i,j) ) && !Ydone )
    { *(*pGradY)(i,j) = ( *Input(i,j+1) - *Input(i,j) )/dy; Ydone = true; }
    if( !Ydone )
    { *(*pGradY)(i,j) = 0.0; }  
   }
  }
  
 }
 return true;
}

// this one assumes that the one-sided gradients have already been computed 

bool ExtrapolateInteriorSpeed( Matrix& LevelSet, Matrix* Input )
{
 extern double _EPS;

 extern Matrix X;
 extern Matrix Y;

 // flag the necessary points
 int** FlaggedPoints; 
 FlaggedPoints = new int* [LevelSet.TellRows()];
 for( int k=0 ; k < LevelSet.TellRows() ; k++ )
 {
  FlaggedPoints[k] = new int [LevelSet.TellCols()]; 
 }
 
 int FlaggedPointCount = 0;
 
 int i,j;
 for( i=1 ; i < LevelSet.TellRows()-1 ; i++ )
 {
  for( j=1 ; j < LevelSet.TellCols()-1 ; j++ )
  {
   FlaggedPoints[i][j] = IsNextToInterface(LevelSet,i,j);
   if( FlaggedPoints[i][j] != 0 )
   { FlaggedPointCount++; }
  }
 }  

 for( i=1 ; i < LevelSet.TellRows()-1 ; i++ )
 {
  for( j=1 ; j < LevelSet.TellCols()-1 ; j++ )
  {
   
   int DirectionX = 0;
   int DirectionY = 0;
/*
   if( FlaggedPoints[i][j] == 1 )
   { DirectionX = 1; DirectionY = 0; }
   if( FlaggedPoints[i][j] == 2 )
   { DirectionX = 1; DirectionY = 1; }
   if( FlaggedPoints[i][j] == 3 )
   { DirectionX = 0; DirectionY = 1; }
   if( FlaggedPoints[i][j] == 4 )
   { DirectionX = -1; DirectionY = 1; }
   if( FlaggedPoints[i][j] == 5 )
   { DirectionX = -1; DirectionY = 0; }
   if( FlaggedPoints[i][j] == 6 )
   { DirectionX = -1; DirectionY = -1; }
   if( FlaggedPoints[i][j] == 7 )
   { DirectionX = 0; DirectionY = -1; }
   if( FlaggedPoints[i][j] == 8 )
   { DirectionX = 1; DirectionY = -1; }
*/
   if( FlaggedPoints[i][j] != 0 )
   {
    GeometricDataPacket GDP = AdaptiveGeometricData(X,Y,LevelSet,*X(i),*Y(j));
	
	if( GDP.NormalX > _EPS )
	{ DirectionX = 1; }
	if( GDP.NormalX < -_EPS )
	{ DirectionX = -1; }
	if( GDP.NormalY > _EPS )
	{ DirectionY = 1; }
	if( GDP.NormalY < -_EPS )
	{ DirectionY = -1; }
	
	if( fabs( GDP.NormalX ) <= 0.5 )
	{ DirectionX = 0; }
	if( fabs( GDP.NormalY ) <= 0.5 )
	{ DirectionY = 0; }
  
/*  
	if( fabs( GDP.NormalX ) > fabs( GDP.NormalY ) + _EPS )
	{ DirectionY = 0; }
	if( fabs( GDP.NormalY ) > fabs( GDP.NormalX ) + _EPS )
	{ DirectionX = 0; }
*/
   }
   
   if( DirectionX != 0 || DirectionY != 0 )
   {
    DirectionX *= -1;
	DirectionY *= -1;
   
    int NumberOfPoints = 0;
    int k=1;
    while( k <= 4 )
    {
     if( *LevelSet(i+k*DirectionX,j+k*DirectionY) <= _EPS )
 	 { NumberOfPoints++; } 
	 else
	 { k = 5; }
	 k++;
    }

	if( NumberOfPoints )
	{
	 Matrix Xpoints = zeros(1,NumberOfPoints );
     Matrix Ypoints = zeros(1,NumberOfPoints );
    
     for( k=0 ; k < NumberOfPoints ; k++ )
     {
      *Xpoints(k) = k+1;
	  *Ypoints(k) = *(*Input)(i+(k+1)*DirectionX,j+(k+1)*DirectionY);
     }
     Polynomial Curve = LeastSquaresPolynomialFit(Xpoints,Ypoints,NumberOfPoints-1);   
	 *(*Input)(i,j) = Curve(0.0);
    }	 

   }
   
  }
 }  
 
 // free the memory
 for( int k=0 ; k < LevelSet.TellRows() ; k++ )
 {
  delete [] FlaggedPoints[k];
 }
 delete [] FlaggedPoints;

 return true;
}
